/*
 * imu.c
 *
 *  Created on: Apr 23, 2013
 *      Author: Daniel
 */

#include "DSP28x_Project.h"
#include "imu.h"


void InitIMU(void) {
	InitIMU_Offsets();
	InitIMU_SMPLRT_DIV();
	InitIMU_DLPF_FS();
	InitIMU_AUX_BURST_ADDR();
	return;
}

void InitIMU_Offsets(void) {
	int i;
	do {
		I2caRegs.I2CSTR.all = I2C_CLR_NACK_BIT;		// clear the NACK bit
		while(I2caRegs.I2CSTR.bit.BB == 1);			// wait until the bus is no longer busy
		I2caRegs.I2CSAR = IMU_I2C_Address;			// tell the I2C peripheral where we're transmitting
		I2caRegs.I2CCNT = IMU_NUMBYTES_OFFSETS;		// tell the I2C peripheral how many bytes we're transmitting

		for(i = 0; i < IMU_NUMBYTES_OFFSETS; i++) {
			while(I2caRegs.I2CSTR.bit.XRDY == 0);	// wait until the previous byte has moved to the shift register
													// before sending the next one
			switch(i) {
			case 0:		I2caRegs.I2CDXR = IMU_ADDR_X_OFFS_USRH;		break;		// send the address of the first register
			case 1:		I2caRegs.I2CDXR = IMU_INIT_X_OFFS_USRH;		break;		// each of these bytes is written in sequence
			case 2:		I2caRegs.I2CDXR = IMU_INIT_X_OFFS_USRL;		break;		// starting at the register address (first byte sent)
			case 3:		I2caRegs.I2CDXR = IMU_INIT_Y_OFFS_USRH;		break;
			case 4:		I2caRegs.I2CDXR = IMU_INIT_Y_OFFS_URSL;		break;
			case 5:		I2caRegs.I2CDXR = IMU_INIT_Z_OFFS_USRH;		break;
			case 6:		I2caRegs.I2CDXR = IMU_INIT_Z_OFFS_USRL;		break;
			}
		}

		I2caRegs.I2CMDR.all = 0x6E20;				// send the START condition

		while(I2caRegs.I2CMDR.bit.STP == 1);		// wait for the transmission to finish
													// or for a NACK to occur
		asm("    RPT #255 || NOP");					// if there was an NACK this makes sure the bit is set before we poll it
	} while(I2caRegs.I2CSTR.bit.NACK == 1);			// if there was a NACK, send the message again
	return;
}

void InitIMU_SMPLRT_DIV(void) {
	int i;
	do {
		I2caRegs.I2CSTR.all = I2C_CLR_NACK_BIT;		// clear the NACK bit
		while(I2caRegs.I2CSTR.bit.BB == 1);			// wait until the buss is no longer busy
		I2caRegs.I2CSAR = IMU_I2C_Address;			// tell the I2C peripheral where we're transmitting
		I2caRegs.I2CCNT = IMU_NUMBYTES_SMPLRT_DIV;	// tell the I2C peripheral how many bytes we're transmitting

		for(i = 0; i < IMU_NUMBYTES_SMPLRT_DIV; i++) {
			while(I2caRegs.I2CSTR.bit.XRDY == 0);	// wait until the previous byte has moved to the shift register
													// before sending the next one
			switch(i) {
			case 0:		I2caRegs.I2CDXR = IMU_ADDR_SMPLRT_DIV;		break;
			case 1:		I2caRegs.I2CDXR = IMU_INIT_SMPLRT_DIV;		break;
			}
		}

		I2caRegs.I2CMDR.all = 0x6E20;				// send the START condition

		while(I2caRegs.I2CMDR.bit.STP == 1);		// wait for the transmission to finish
													// or for a NACK to occur
		asm("    RPT #255 || NOP");					// if there was an NACK this makes sure the bit is set before we test it
	} while(I2caRegs.I2CSTR.bit.NACK == 1);			// if there was a NACK, send the message again
	return;
}

void InitIMU_DLPF_FS(void) {
	int i;
	do {
		I2caRegs.I2CSTR.all = I2C_CLR_NACK_BIT;		// clear the NACK bit
		while(I2caRegs.I2CSTR.bit.BB == 1);			// wait until the buss is no longer busy
		I2caRegs.I2CSAR = IMU_I2C_Address;			// tell the I2C peripheral where we're transmitting
		I2caRegs.I2CCNT = IMU_NUMBYTES_DLPF_FS;	// tell the I2C peripheral how many bytes we're transmitting

		for(i = 0; i < IMU_NUMBYTES_DLPF_FS; i++) {
			while(I2caRegs.I2CSTR.bit.XRDY == 0);	// wait until the previous byte has moved to the shift register
													// before sending the next one
			switch(i) {
			case 0:		I2caRegs.I2CDXR = IMU_ADDR_DLPF_FS;		break;
			case 1:		I2caRegs.I2CDXR = IMU_INIT_DLPF_FS;		break;
			}
		}

		I2caRegs.I2CMDR.all = 0x6E20;				// send the START condition

		while(I2caRegs.I2CMDR.bit.STP == 1);		// wait for the transmission to finish
													// or for a NACK to occur
		asm("    RPT #255 || NOP");					// if there was an NACK this makes sure the bit is set before we test it
	} while(I2caRegs.I2CSTR.bit.NACK == 1);			// if there was a NACK, send the message again
	return;
}

void InitIMU_AUX_BURST_ADDR(void) {
	int i;
	do {
		I2caRegs.I2CSTR.all = I2C_CLR_NACK_BIT;		// clear the NACK bit
		while(I2caRegs.I2CSTR.bit.BB == 1);			// wait until the buss is no longer busy
		I2caRegs.I2CSAR = IMU_I2C_Address;			// tell the I2C peripheral where we're transmitting
		I2caRegs.I2CCNT = IMU_NUMBYTES_AUX_BURST_ADDR;	// tell the I2C peripheral how many bytes we're transmitting

		for(i = 0; i < IMU_NUMBYTES_AUX_BURST_ADDR; i++) {
			while(I2caRegs.I2CSTR.bit.XRDY == 0);	// wait until the previous byte has moved to the shift register
													// before sending the next one
			switch(i) {
			case 0:		I2caRegs.I2CDXR = IMU_ADDR_AUX_BURST_ADDR;		break;
			case 1:		I2caRegs.I2CDXR = IMU_INIT_AUX_BURST_ADDR;		break;
			}
		}

		I2caRegs.I2CMDR.all = 0x6E20;				// send the START condition

		while(I2caRegs.I2CMDR.bit.STP == 1);		// wait for the transmission to finish
													// or for a NACK to occur
		asm("    RPT #255 || NOP");					// if there was an NACK this makes sure the bit is set before we test it
	} while(I2caRegs.I2CSTR.bit.NACK == 1);			// if there was a NACK, send the message again
	return;
}

void IMU_read_all(Uint16 * results) {
	int i;
	do {
		I2caRegs.I2CSTR.all = I2C_CLR_NACK_BIT;
		while(I2caRegs.I2CSTR.bit.BB == 1);
		I2caRegs.I2CSAR = IMU_I2C_Address;
		I2caRegs.I2CCNT = IMU_NUMBYTES_POLL;

		while(I2caRegs.I2CSTR.bit.XRDY == 0);
		I2caRegs.I2CDXR = IMU_ADDR_TEMP_OUT_H;

		I2caRegs.I2CMDR.all = 0x2620;				// issue a START command (no stop)

		while(I2caRegs.I2CSTR.bit.ARDY) {			// wait for the transmission to finish
			if(I2caRegs.I2CSTR.bit.NACK == 1) {		// if we get a NACK during this transmission
				I2caRegs.I2CMDR.bit.STP = 1;		// issue a STOP
				break;
			}
		}

	} while(I2caRegs.I2CSTR.bit.NACK == 1);

	while(I2caRegs.I2CSTR.bit.BB == 1);
	I2caRegs.I2CSAR = IMU_I2C_Address;
	I2caRegs.I2CCNT = IMU_NUMBYTES_READ_ALL;
	I2caRegs.I2CMDR.all = 0x2C20;					// issue a restart command

	while(I2caRegs.I2CMDR.bit.STP == 1);			// wait for the slave to finish transmitting


	for(i = 0; i < IMU_NUMBYTES_READ_ALL; i++) {
		while(I2caRegs.I2CSTR.bit.RRDY == 0);
		results[i] = I2caRegs.I2CDRR;
	}
}
